#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <vector>
#include <opencv2/opencv.hpp>
//#include <Eigen/Eigen>
#include "dragon.h"
#include <opencv2/opencv.hpp>
#include "sparsekdtree.h"
const int samplesize = 50000;
const int cubesize = 32;
float thresold = 0;//蒙
void calSum(Eigen::SparseMatrix<unsigned char, Eigen::RowMajor>& E, SparseKDTree& tree, int father_id )
{
    if(tree.Nodes[father_id-1].idx<0){
        //如果遇到了叶子-1
        if(tree.Nodes[father_id-1].same.size()){
            tree.Nodes[father_id-1].Es = E.row(std::abs(tree.Nodes[father_id-1].idx)).cast<float>();
            tree.Nodes[father_id-1].Es = tree.Nodes[father_id-1].Es * tree.Nodes[father_id-1].same.size();
            //计算权重坐标
            for(int i=0;i<tree.Nodes[father_id-1].same.size();i++){
                int cursameid = tree.Nodes[father_id-1].same[i]-1;
                tree.Nodes[father_id-1].Pv.coeffRef(0,0) += vertices[cursameid][0];
                tree.Nodes[father_id-1].Pv.coeffRef(1,0) += vertices[cursameid][1];
                tree.Nodes[father_id-1].Pv.coeffRef(2,0) += vertices[cursameid][2];
            }
            tree.Nodes[father_id-1].Pv.coeffRef(0,0) /= tree.Nodes[father_id-1].same.size();
            tree.Nodes[father_id-1].Pv.coeffRef(1,0) /= tree.Nodes[father_id-1].same.size();
            tree.Nodes[father_id-1].Pv.coeffRef(2,0) /= tree.Nodes[father_id-1].same.size();
        }
        else{
            tree.Nodes[father_id-1].Es = E.row(std::abs(tree.Nodes[father_id-1].idx)).cast<float>();
            tree.Nodes[father_id-1].Pv.coeffRef(0,0) = vertices[std::abs(tree.Nodes[father_id-1].idx)-1][0];
            tree.Nodes[father_id-1].Pv.coeffRef(1,0) = vertices[std::abs(tree.Nodes[father_id-1].idx)-1][1];
            tree.Nodes[father_id-1].Pv.coeffRef(2,0) = vertices[std::abs(tree.Nodes[father_id-1].idx)-1][2];
        }
        tree.Nodes[father_id-1].AreaMax = E.row(std::abs(tree.Nodes[father_id-1].idx)-1).cast<float>();
        tree.Nodes[father_id-1].AreaMin = E.row(std::abs(tree.Nodes[father_id-1].idx)-1).cast<float>();
        return ;
    }
    calSum(E, tree, tree.Nodes[father_id-1].left);
    calSum(E, tree, tree.Nodes[father_id-1].right);
    int leftchild = tree.Nodes[father_id-1].left,
            rightchild = tree.Nodes[father_id-1].right;
    tree.Nodes[father_id-1].Es = tree.Nodes[leftchild -1].Es + tree.Nodes[rightchild -1].Es;
    for(int i=0;i<6*32*32;i++){
        tree.Nodes[father_id-1].AreaMax.coeffRef(0,i) = tree.Nodes[leftchild -1].AreaMax.coeffRef(0,i)>=tree.Nodes[rightchild - 1].AreaMax.coeffRef(0,i)?
                    tree.Nodes[leftchild -1].AreaMax.coeffRef(0,i):tree.Nodes[rightchild - 1].AreaMax.coeffRef(0,i);
        tree.Nodes[father_id-1].AreaMin.coeffRef(0,i) = tree.Nodes[leftchild -1].AreaMin.coeffRef(0,i)<=tree.Nodes[rightchild - 1].AreaMin.coeffRef(0,i)?
                    tree.Nodes[leftchild -1].AreaMin.coeffRef(0,i):tree.Nodes[rightchild - 1].AreaMin.coeffRef(0,i);
    }
    if(tree.Nodes[leftchild -1].idx<0&&tree.Nodes[rightchild - 1].idx<0){
        //左右都是叶子；
        for(int i=0;i<6*32*32;i++)
            if(tree.Nodes[father_id-1].Es.coeffRef(0,i)==0)
                continue;
            else if(tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&!tree.Nodes[rightchild - 1].Es.coeffRef(0,i)){
                //左边不是0但右边是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[leftchild -1].Pv.coeffRef(0,0);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[leftchild -1].Pv.coeffRef(1,0);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[leftchild -1].Pv.coeffRef(2,0);
                continue;
            }
            else if(!tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&tree.Nodes[rightchild -1].Es.coeffRef(0,i)){
                //右边不是0但左边是0
                //如果右边系个无细路既叶子
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(0,0);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(1,0);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(2,0);
                continue;
            }
            else{
                //左右都是Es都不是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(0,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(0,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(1,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(1,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(2,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(2,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
            }
    }
    else if(tree.Nodes[leftchild -1].idx>=0&&tree.Nodes[rightchild - 1].idx<0){
        //左边不是叶子，右边是叶子
        for(int i=0;i<6*32*32;i++)
            if(tree.Nodes[father_id-1].Es.coeffRef(0,i)==0)
                continue;
            else if(tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&!tree.Nodes[rightchild - 1].Es.coeffRef(0,i)){
                //左边不是0但右边是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[leftchild -1].Pv.coeffRef(0,i);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[leftchild -1].Pv.coeffRef(1,i);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[leftchild -1].Pv.coeffRef(2,i);
                continue;
            }
            else if(!tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&tree.Nodes[rightchild -1].Es.coeffRef(0,i)){
                //右边不是0但左边是0
                //如果右边系个无细路既叶子
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(0,0);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(1,0);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(2,0);
                continue;
            }
            else{
                //左右都是Es都不是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(0,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(0,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(1,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(1,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(2,0)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(2,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
            }


    }
    else if(tree.Nodes[leftchild -1].idx<0&&tree.Nodes[rightchild - 1].idx<0){
        //左是叶子, 右不是叶子；
        for(int i=0;i<6*32*32;i++)
            if(tree.Nodes[father_id-1].Es.coeffRef(0,i)==0)
                continue;
            else if(tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&!tree.Nodes[rightchild - 1].Es.coeffRef(0,i)){
                //左边不是0但右边是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[leftchild -1].Pv.coeffRef(0,0);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[leftchild -1].Pv.coeffRef(1,0);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[leftchild -1].Pv.coeffRef(2,0);
                continue;
            }
            else if(!tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&tree.Nodes[rightchild -1].Es.coeffRef(0,i)){
                //右边不是0但左边是0
                //如果右边系个无细路既叶子
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(0,i);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(1,i);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(2,i);
                continue;
            }
            else{
                //左右都是Es都不是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(0,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(0,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(1,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(1,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(2,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(2,0)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
            }
    }

    else if(tree.Nodes[leftchild -1].idx>=0&&tree.Nodes[rightchild - 1].idx>=0){
        //左边不是叶子，右边也不是叶子
        for(int i=0;i<6*32*32;i++)
            if(tree.Nodes[father_id-1].Es.coeffRef(0,i)==0)
                continue;
            else if(tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&!tree.Nodes[rightchild - 1].Es.coeffRef(0,i)){
                //左边不是0但右边是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[leftchild -1].Pv.coeffRef(0,i);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[leftchild -1].Pv.coeffRef(1,i);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[leftchild -1].Pv.coeffRef(2,i);
                continue;
            }
            else if(!tree.Nodes[leftchild -1].Es.coeffRef(0,i)&&tree.Nodes[rightchild -1].Es.coeffRef(0,i)){
                //右边不是0但左边是0
                //如果右边系个无细路既叶子
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(0,i);
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(1,i);
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = tree.Nodes[rightchild - 1].Pv.coeffRef(2,i);
                continue;
            }
            else{
                //左右都是Es都不是0
                tree.Nodes[father_id-1].Pv.coeffRef(0,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(0,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(0,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(1,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(1,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(1,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
                tree.Nodes[father_id-1].Pv.coeffRef(2,i) = (tree.Nodes[rightchild - 1].Pv.coeffRef(2,i)*tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+
                        tree.Nodes[leftchild -1].Pv.coeffRef(2,i)*tree.Nodes[leftchild -1].Es.coeffRef(0,i))/(tree.Nodes[rightchild - 1].Es.coeffRef(0,i)+tree.Nodes[leftchild -1].Es.coeffRef(0,i));
            }
    }
    return;
}
const int PI = 3.1415;
float getR(float* x0, float* x1)
{
    static float yita = 1.5;
    static float sigma_s = 0.75, sigma_a = 0.02;
    static float g=0.25;//散射角平均cos
    static float sigma_s2 = (1-g)*sigma_s, sigma_t2 = sigma_a+sigma_s2,
            alpha2 = sigma_s2/sigma_t2, sigma_tr = sqrt(3*sigma_a*sigma_t2), z_r = 1/sigma_t2,
            F_dr = -1.440/yita*yita+0.710/yita+0.668+0.0636*yita, A = (1+F_dr)*(1-F_dr);
    float r = sqrt(pow(x1[0]-x0[0], 2)+
            pow(x1[1]-x0[1], 2)+
            pow(x1[2]-x0[2], 2));
    float d_r = sqrt(r*r + z_r*z_r),z_v = z_r*(1+4*A/3), d_v = sqrt(r*r +z_v*z_v);
    float R;
    R = alpha2/(4*PI)*(z_r*(sigma_tr+1/d_r)*exp(-sigma_tr*d_r)/d_r/d_r +
                       z_v*(sigma_tr+1/d_v)*exp(-sigma_tr*d_v)/d_v/d_v);
    return R;
}
float SearchTree(float curx[], Eigen::SparseMatrix<unsigned char, Eigen::RowMajor>& E, int fatherid, int w, int x0, bool ownX, SparseKDTree& tree)
{
    myNode fathernode = tree.Nodes[fatherid - 1];
    if(fathernode.Es.coeffRef(0,w)==0)//当前子树当前维度的irradiance和为0，毫无贡献，毫无长进要它干啥
        return 0;
    if(fathernode.idx<0){
        //突破重围居然到了叶子。。。看来是个离x0很近的点，直接算
        float R = getR(curx, vertices[abs(fathernode.idx)-1]);
        return R*fathernode.Es.coeffRef(0,w);
    }
    //如果x在当前左子树，继续分左边
    if(ownX && E.coeffRef(x0,fathernode.idx)<=fathernode.boundary){
        return SearchTree(curx, E, fathernode.right, w,x0, false, tree) +
                SearchTree(curx, E, fathernode.left, w, x0, true, tree);
    }
    else if(ownX && E.coeffRef(x0,fathernode.idx)>fathernode.boundary){
        return SearchTree(curx, E, fathernode.right, w,x0, true, tree) +
                SearchTree(curx, E, fathernode.left, w, x0, false, tree);

    }
    //如果当前小于阈值，返回R
    float www =1.0f;
    Eigen::SparseMatrix<float, Eigen::RowMajor> area = fathernode.AreaMax-fathernode.AreaMin;
    for(int i=0;i<6*32*32;i++){
        www*= area.coeffRef(0,i);//区域
    }
    www = www/(sqrt(pow(curx[0]-fathernode.Pv.coeffRef(0,w),2)+
               pow(curx[1]-fathernode.Pv.coeffRef(1,w),2)+
            pow(curx[2]-fathernode.Pv.coeffRef(2,w),2)
               ));
    if(www<thresold){
        //小于积分项阈值，计算
        float xi[3];
        xi[0] = fathernode.Pv.coeffRef(0,w);
        xi[1] = fathernode.Pv.coeffRef(1,w);
        xi[2] = fathernode.Pv.coeffRef(2,w);
        float R = getR(curx, xi);
        return R*fathernode.Es.coeffRef(0,w);
    }
    return SearchTree(curx, E, fathernode.right, w,x0, false, tree) +
            SearchTree(curx, E, fathernode.left, w, x0, false, tree);
}

void getT(Eigen::SparseMatrix<float, Eigen::RowMajor>& T, Eigen::SparseMatrix<unsigned char, Eigen::RowMajor>& E, SparseKDTree& tree)
{
    for(int i=0;i<T.rows();i++){
        float curx[3];
        curx[0] = vertices[i][0];
        curx[1] = vertices[i][1];
        curx[2] = vertices[i][2];
        for(int j=0;j<6*32*32;j++){
            //对于每个T(x,w)遍历那个树。。很傻比。。
            T.coeffRef(i,j) = SearchTree(curx, E, 1, j,i,true, tree);
        }
    }
}
int main()
{
    cv::Mat curE(samplesize, 128,CV_8UC1);
    std::ifstream in("waveE.txt", std::ios::binary);
    std::cout<<"open"<<std::endl;
    in.read((char*)curE.data, curE.rows * curE.cols * sizeof(uchar));
    //std::cout<<curE.row(0)<<std::endl;
    cv::Mat maxid(samplesize, 128, CV_32SC1);
    std::ifstream in2("maxid.txt", std::ios::binary);
    std::cout<<"open"<<std::endl;
    in2.read((char*)maxid.data, maxid.rows * maxid.cols * sizeof(int));
    std::cout<<maxid.row(0)<<std::endl;
    Eigen::SparseMatrix<unsigned char, Eigen::RowMajor> sparseE_(samplesize,6*32*32);
    for(int i=0;i<samplesize;i++){
        int* p = maxid.ptr<int>(i);
        uchar*p1 = curE.ptr<uchar>(i);
        for(int j=0;j<128;j++){
            sparseE_.coeffRef(i,*p) = *p1;
            p++;
            p1++;
        }
    }
    maxid.release();
//    cv::Mat vertices_mat(50000, 3, CV_32FC1, vertices);
//    cv::KDTree tree(vertices_mat);
   // Eigen::SparseMatrix<unsigned char, Eigen::ColMajor> sparseE = sparseE_;
    /*SparseKDTree tree(sparseE, 6*32*32);
    std::cout<<"build finish"<<std::endl;
    std::ofstream out("tree.txt",std::ios::binary);
    int balala = tree.Nodes.size();
    out.write((char*)&balala, sizeof(int));
    for(int i=0;i<tree.Nodes.size();i++){
        myNode curnode = tree.Nodes[i];
        out.write((char*)&(curnode.idx), sizeof(int));
        out.write((char*)&(curnode.left), sizeof(int));
        out.write((char*)&(curnode.right), sizeof(int));
        out.write((char*)&(curnode.boundary), sizeof(int));
        int hhh = curnode.same.size();
        out.write((char*)&hhh, sizeof(int));
        for(int j=0;j<curnode.same.size();j++){
            out.write((char*)&(curnode.same[j]), sizeof(int));
        }
    }
    std::cout<<tree.Nodes.size()<<std::endl;*/
    SparseKDTree tree;
    std::ifstream in_tree("tree.txt",std::ios::binary);
    int tree_node_size;
    in_tree.read((char*)&tree_node_size, sizeof(int));
    for(int i=0;i<tree_node_size;i++){
        myNode curnode;
        in_tree.read((char*)&(curnode.idx), sizeof(int));
        in_tree.read((char*)&(curnode.left), sizeof(int));
        in_tree.read((char*)&(curnode.right), sizeof(int));
        in_tree.read((char*)&(curnode.boundary), sizeof(int));
        int curnode_same_size;
        in_tree.read((char*)&curnode_same_size, sizeof(int));
        for(int j=0;j<curnode_same_size;j++){
            int balala;
            in_tree.read((char*)&balala, sizeof(int));
            curnode.same.push_back(balala);
        }
        tree.Nodes.push_back(curnode);
    }
    //递归
    calSum(sparseE_, tree, 1);
    Eigen::SparseMatrix<float, Eigen::RowMajor> T(samplesize,6*32*32);
    getT(T,sparseE_, tree);
    std::ofstream out3("T.txt",std::ios::binary);
    std::ofstream out2("T_index.txt",std::ios::binary);
    for(int i=0;i<T.rows();i++){
        for(int j=0;j<6*32*32;j++){
            if(!T.coeffRef(i,j))
                continue;
            float index = i*T.rows()+j;
            out3.write((char*)&T.coeffRef(i,j), sizeof(float));
            out2.write((char*)&index, sizeof(float));
        }
    }
    std::cout<<"output file is finished"<<std::endl;

}
